/**
 * @file LibCodeReleaseProvider.cpp
 */

#include "LibCodeReleaseProvider.h"

MAKE_MODULE(LibCodeReleaseProvider, behaviorControl);
using namespace Transformation;

void LibCodeReleaseProvider::update(LibCodeRelease& libCodeRelease)
{
  libCodeRelease.timeSinceBallWasSeen = theFrameInfo.getTimeSince(theBallModel.timeWhenLastSeen);
  libCodeRelease.angleToGoal = (theRobotPose.inversePose * Vector2f(theFieldDimensions.xPosOpponentGroundline, 80.f)).angle();
  libCodeRelease.between = [&](float value, float min, float max) -> bool
  {
    return value >= min && value <= max;
  };
libCodeRelease.angleToCenter=(theRobotPose.inversePose * Vector2f(0.f, 0.f)).angle();
  libCodeRelease.whistleTime = theWhistle.lastTimeWhistleDetected;
 /* libCodeRelease.angleToPenaltyPoint = (theRobotPose.inversePose * Vector2f(1300.f, 0.f)).angle();//add by BLB
  l
  
  libCodeRelease.angleToOpponentKeeperLeft= (theRobotPose.inversePose * Vector2f(theFieldDimensions.xPosOpponentGroundline, 400.f)).angle();
  libCodeRelease.angleToOpponentKeeperRight= (theRobotPose.inversePose * Vector2f(theFieldDimensions.xPosOpponentGroundline, -400.f)).angle();
  
  libCodeRelease.distanceToOpponentKeeperLeft = robotToField(theRobotPose.inversePose, Vector2f(theFieldDimensions.xPosOpponentGroundline, 400.f)).norm();
  libCodeRelease.distanceToOpponentKeeperRight = robotToField(theRobotPose.inversePose, Vector2f(theFieldDimensions.xPosOpponentGroundline, -400.f)).norm();
  
  libCodeRelease.angleToBall = (theRobotPose.inversePose * Vector2f(robotToField(theRobotPose,theBallModel.estimate.position))).angle();
  
  libCodeRelease.angleToStrikerLeft = (theRobotPose.inversePose * Vector2f(2000.f, 750.f)).angle();
  libCodeRelease.angleToStrikerRight = (theRobotPose.inversePose * Vector2f(2000.f, -750.f)).angle(); 
  
  libCodeRelease.distanceToStrikerLeft = robotToField(theRobotPose.inversePose, Vector2f(2000.f, 750.f)).norm();
  libCodeRelease.distanceToStrikerRight = robotToField(theRobotPose.inversePose, Vector2f(2000.f, -750.f)).norm();*/

	static float odometryR = 0.f;
	static float odometryX = 0.f;
        static float odometryY = 0.f;
	odometryR = static_cast<float>(theOdometer.odometryOffset.rotation);
	odometryX = theOdometer.odometryOffset.translation.x();
        odometryY = theOdometer.odometryOffset.translation.y();
	if (theGameInfo.state == STATE_PLAYING && theRobotInfo.penalty == PENALTY_NONE)
	{
		libCodeRelease.odometryRSum += odometryR;
		libCodeRelease.odometryXSum += odometryX;
                libCodeRelease.odometryYSum += odometryY;
	}
	else
	{
		libCodeRelease.odometryRSum = 0.f;
		libCodeRelease.odometryXSum = 0.f;
                libCodeRelease.odometryYSum = 0.f;
	}
  //libCodeRelease.distanceToGoal = robotToField(theRobotPose.inversePose, Vector2f(4400.f, 0.f)).norm();
  //libCodeRelease.angleToSupporter = (theTeamData.teammates[1].theRobotPose.inversePose * Vector2f(2000.f, -750.f)).angle();
  //add by zzp
	if(theTeamData.numberOfActiveTeammates>0)
	{
		for(int i = 0; i < (int)theTeamData.teammates.size(); i++)
		{
		  if(theTeamData.teammates[i].number == 2)
		  {
		    /*libCodeRelease.angleToStriker = (theRobotPose.inversePose * theTeamData.teammates[i].theRobotPose.translation).angle();
		    libCodeRelease.distanceToStriker = robotToField(theRobotPose.inversePose, theTeamData.teammates[i].theRobotPose.translation).norm();
		    libCodeRelease.distanceToGoalfromStriker = robotToField(theTeamData.teammates[i].theRobotPose.translation,Vector2f(4400.f, 0.f)).norm();*/
		    if(theTeamData.teammates[i].status == 2)
		    {
					libCodeRelease.StrikerStatus = true;
		    }
		    else
		    {
					libCodeRelease.StrikerStatus = false;
		    }
		  }
		  if(theTeamData.teammates[i].number == 3)
		  {
		    /*libCodeRelease.angleToSupporter = (theRobotPose.inversePose * theTeamData.teammates[i].theRobotPose.translation).angle();
		    libCodeRelease.distanceToSupporter = robotToField(theRobotPose.inversePose, theTeamData.teammates[i].theRobotPose.translation).norm();
		    libCodeRelease.distanceToGoalfromSupporter = robotToField(theTeamData.teammates[i].theRobotPose.translation,Vector2f(4400.f, 0.f)).norm();*/
		    if(theTeamData.teammates[i].status == 2)
		    {
					libCodeRelease.SupporterStatus = true;
		    }
		    else
		    {
					libCodeRelease.SupporterStatus = false;
		    }
		  }
		}
}
}
